/*
 * logic_fast.c
 *
 *  Created on: 2013.01.09.
 *      Author: Peter
 */

#include "stm32f4xx.h"
#include "drive_control.h"
#include "steering_control.h"
#include "steering.h"
#include "Inertial.h"
#include "LineTracker.h"
#include "Rotary.h"
#include "logic_fast.h"
#include "SharpSensors.h"


uint32_t fst_wall;
uint32_t fst_state = STATE_FST_START;
uint32_t fst_wall_both_detected = 0;
uint32_t fst_three_line_suspected = 0;
uint32_t fst_lap = 0;
uint32_t fst_lap_suspected = 0;
uint32_t fst_line_lost  = 0;
uint32_t fst_cross_line_detected = 0;
uint32_t fst_time = 0;
uint32_t fst_scnd_line_suspect = 0;
float linepos;
float test_line_idx = 0.0;

void LogicFast()
{
	uint8_t line_idx = 0;
	float mindiff = 100;
	uint8_t i;
	if(LineArray[0].linewidth > 20)
	{
		if(fst_lap_suspected < 2)
		{
			fst_lap_suspected++;
		}
		else
		{
			fst_cross_line_detected = 1;
			fst_lap_suspected = 5;
		}
	} else
	{
		if(fst_lap_suspected > 0) fst_lap_suspected--;
		else if(fst_cross_line_detected == 1)
		{
			fst_cross_line_detected = 0;
			fst_lap++;
			if(fst_lap == 4)
			{
				fst_time = 0;
				fst_state = STATE_STOP;
			}
		}
	}

	if(LineArray[1].valid)
	{
		/*if(LineArray[2].valid)
		{
			linepos = (LineArray[0].linepos + LineArray[1].linepos + LineArray[2].linepos)/3;
		}
		else
		{
			linepos = (LineArray[0].linepos + LineArray[1].linepos)/2;
		}
		for(i = 0; i<3; i++)
		{
			if((LineArray[i].valid == 1) && (ABS(LineArray[i].linepos - 11.5) < mindiff))
			{
				mindiff = ABS(LineArray[i].linepos - 11.5);
				linepos = LineArray[i].linepos;
			}
		}*/
		if(LineArray[2].valid)linepos = LineArray[1].linepos;
		else linepos = (LineArray[0].linepos + LineArray[1].linepos)/2.0;

		line_idx = 1;
		fst_scnd_line_suspect = 1;
	}
	else
	{
		if(LineArray[0].valid)
		{
			linepos = LineArray[0].linepos;
			if(fst_scnd_line_suspect == 0)
			{
				line_idx = 0;
			} else
			{
				fst_scnd_line_suspect = 0;
				line_idx = 3;
			}
			if(fst_line_lost > 0) fst_line_lost--;
		}
		else if(fst_line_lost < 10)
		{

			linepos = -1;
			fst_line_lost++;
		}
		else
		{
			//fst_state = STATE_EMERGENCY_STOP; /*TODO: Remove or increase threshold*/
		}
	}
	test_line_idx = line_idx;
	//--------------------------------------------------------/
	switch(fst_state)
	{
	case STATE_FST_START:
		fst_time++;
		if(fst_time == 100)
		{
			fst_time = 0;
			fst_state = STATE_CURVE_SLOW;
		}
	break;
	case STATE_FAST:
		if (LineArray[0].valid ==1)
		{
			if(LineArray[1].valid ==1)
			{
				Steering_Control(11.5f, linepos,30,0,0);
				if(fst_scnd_line_suspect < 2)fst_scnd_line_suspect++;
			} else
			{
				if(fst_scnd_line_suspect > 0)
				{
					fst_scnd_line_suspect--;
					Steering_Control(11.5f, linepos,30,0,0);
				}
				else Steering_Control(11.5f, linepos,30,0,18*speed);
			}
		}
		//else fst_state = STATE_EMERGENCY_STOP;
		Drive_ControlFast(SPEED_FAST,speed);
		if((LineArray[0].valid == 1) && (LineArray[1].valid == 1) && (LineArray[2].valid == 1))
		{
			if(fst_three_line_suspected < 3)
			{
				fst_three_line_suspected++;
			} else
			{
				fst_three_line_suspected = 0;
				fst_state = STATE_CURVE_SLOW;
			}
		} else
		{
			if(fst_three_line_suspected > 0) fst_three_line_suspected--;
		}

	break;
	case STATE_CURVE_SLOW:
		if (LineArray[0].valid ==1)
		{
			if(LineArray[1].valid ==1)
			{
				Steering_Control(11.5f, linepos,30,0,0);
				if(fst_scnd_line_suspect < 2)fst_scnd_line_suspect++;
			} else
			{
				if(fst_scnd_line_suspect > 0)
				{
					fst_scnd_line_suspect--;
					Steering_Control(11.5f, linepos,30,0,0);
				}
				else Steering_Control(11.5f, linepos,110,0,900);
			}
		}
		//else fst_state = STATE_EMERGENCY_STOP;
		Drive_ControlFast(SPEED_CURVE,speed);
		fst_wall = (SharpSensorDistances[LEFT_SENS] < FAST_WALL_DETECT_LIMIT)? FAST_WALL_LEFT : 0;
		if(SharpSensorDistances[RIGHT_SENS] < FAST_WALL_DETECT_LIMIT) fst_wall |= FAST_WALL_RIGHT;
		if(fst_wall == FAST_WALL_BOTH)
		{
			fst_wall_both_detected++;
			if(fst_wall_both_detected > FAST_WALL_DETECT_TIMEOUT)
			{
				fst_wall_both_detected = 0;
				fst_state = STATE_FAST;
			}
		} else
		{
			if(fst_wall_both_detected > 0) fst_wall_both_detected--;
		}
	break;
	case STATE_STOP:
		fst_time++;
		if(fst_time < 25)
		{
			Steering_Control(11.5f, LineArray[0].linepos,110,0,900);
			Drive_ControlFast(SPEED_CURVE,speed);
		}
		else
		{
			Steering_Control(11.5f, LineArray[0].linepos,110,0,900);
			Drive_ControlFast(0,speed);
		}
	break;
	case STATE_EMERGENCY_STOP:
		Drive_ControlFast(0,speed);
		break;
	break;

	}
}
